#ifndef __MATH_MATRIX_H__
#define __MATH_MATRIX_H__

/*
===============================================================================

Matrix classes, all matrices are row-major except Mat3

===============================================================================
*/

#define MATRIX_INVERSE_EPSILON		1e-14f
#define MATRIX_EPSILON				1e-6f

namespace inf {

    class Angles;
    class Quat;
    class CQuat;
    class Rotation;
    class Mat4;

    //===============================================================
    //
    //	Mat2 - 2x2 matrix
    //
    //===============================================================

    class Mat2 {
    public:
        Mat2( void );
        explicit Mat2( const Vec2 &x, const Vec2 &y );
        explicit Mat2( const float xx, const float xy, const float yx, const float yy );
        explicit Mat2( const float src[ 2 ][ 2 ] );

        const Vec2 &	operator[]( int index ) const;
        Vec2 &		operator[]( int index );
        Mat2			operator-() const;
        Mat2			operator*( const float a ) const;
        Vec2			operator*( const Vec2 &vec ) const;
        Mat2			operator*( const Mat2 &a ) const;
        Mat2			operator+( const Mat2 &a ) const;
        Mat2			operator-( const Mat2 &a ) const;
        Mat2 &		operator*=( const float a );
        Mat2 &		operator*=( const Mat2 &a );
        Mat2 &		operator+=( const Mat2 &a );
        Mat2 &		operator-=( const Mat2 &a );

        friend Mat2	operator*( const float a, const Mat2 &mat );
        friend Vec2	operator*( const Vec2 &vec, const Mat2 &mat );
        friend Vec2 &	operator*=( Vec2 &vec, const Mat2 &mat );

        bool			compare( const Mat2 &a ) const;						// exact compare, no epsilon
        bool			compare( const Mat2 &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==( const Mat2 &a ) const;					// exact compare, no epsilon
        bool			operator!=( const Mat2 &a ) const;					// exact compare, no epsilon

        void			zero( void );
        void			identity( void );
        bool			is_identity( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_symmetric( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_diagonal( const float epsilon = MATRIX_EPSILON ) const;

        float			trace( void ) const;
        float			determinant( void ) const;
        Mat2			transpose( void ) const;	// returns transpose
        Mat2 &		    transpose_self( void );
        Mat2			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
        bool			inverse_self( void );		// returns false if determinant is zero
        void			rotation( float angle );
        Mat2			inverse_fast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
        bool			inverse_fast_self( void );	// returns false if determinant is zero

        int				get_dimension( void ) const;
        int				get_square_dimension( void ) const;

        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        static Mat2 ZERO;
        static Mat2 IDENTITY;
    private:
        Vec2			mat[ 2 ];
    };


    INF_INLINE Mat2::Mat2( void ) {
    }

    INF_INLINE Mat2::Mat2( const Vec2 &x, const Vec2 &y ) {
        mat[ 0 ].x = x.x; mat[ 0 ].y = x.y;
        mat[ 1 ].x = y.x; mat[ 1 ].y = y.y;
    }

    INF_INLINE Mat2::Mat2( const float xx, const float xy, const float yx, const float yy ) {
        mat[ 0 ].x = xx; mat[ 0 ].y = xy;
        mat[ 1 ].x = yx; mat[ 1 ].y = yy;
    }

    INF_INLINE Mat2::Mat2( const float src[ 2 ][ 2 ] ) {
        memory_copy( mat, src, 2 * 2 * sizeof( float ) );
    }

    INF_INLINE const Vec2 &Mat2::operator[]( int index ) const {
        //assert( ( index >= 0 ) && ( index < 2 ) );
        return mat[ index ];
    }

    INF_INLINE Vec2 &Mat2::operator[]( int index ) {
        //assert( ( index >= 0 ) && ( index < 2 ) );
        return mat[ index ];
    }

    INF_INLINE Mat2 Mat2::operator-( void ) const {
        return Mat2(	-mat[0][0], -mat[0][1],
            -mat[1][0], -mat[1][1] );
    }

    INF_INLINE Vec2 Mat2::operator*( const Vec2 &vec ) const {
        return Vec2(
            mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y,
            mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y );
    }

    INF_INLINE Mat2 Mat2::operator*( const Mat2 &a ) const {
        return Mat2(
            mat[0].x * a[0].x + mat[0].y * a[1].x,
            mat[0].x * a[0].y + mat[0].y * a[1].y,
            mat[1].x * a[0].x + mat[1].y * a[1].x,
            mat[1].x * a[0].y + mat[1].y * a[1].y );
    }

    INF_INLINE Mat2 Mat2::operator*( const float a ) const {
        return Mat2(
            mat[0].x * a, mat[0].y * a, 
            mat[1].x * a, mat[1].y * a );
    }

    INF_INLINE Mat2 Mat2::operator+( const Mat2 &a ) const {
        return Mat2(
            mat[0].x + a[0].x, mat[0].y + a[0].y, 
            mat[1].x + a[1].x, mat[1].y + a[1].y );
    }

    INF_INLINE Mat2 Mat2::operator-( const Mat2 &a ) const {
        return Mat2(
            mat[0].x - a[0].x, mat[0].y - a[0].y,
            mat[1].x - a[1].x, mat[1].y - a[1].y );
    }

    INF_INLINE Mat2 &Mat2::operator*=( const float a ) {
        mat[0].x *= a; mat[0].y *= a;
        mat[1].x *= a; mat[1].y *= a;

        return *this;
    }

    INF_INLINE Mat2 &Mat2::operator*=( const Mat2 &a ) {
        float x, y;
        x = mat[0].x; y = mat[0].y;
        mat[0].x = x * a[0].x + y * a[1].x;
        mat[0].y = x * a[0].y + y * a[1].y;
        x = mat[1].x; y = mat[1].y;
        mat[1].x = x * a[0].x + y * a[1].x;
        mat[1].y = x * a[0].y + y * a[1].y;
        return *this;
    }

    INF_INLINE Mat2 &Mat2::operator+=( const Mat2 &a ) {
        mat[0].x += a[0].x; mat[0].y += a[0].y;
        mat[1].x += a[1].x; mat[1].y += a[1].y;

        return *this;
    }

    INF_INLINE Mat2 &Mat2::operator-=( const Mat2 &a ) {
        mat[0].x -= a[0].x; mat[0].y -= a[0].y;
        mat[1].x -= a[1].x; mat[1].y -= a[1].y;

        return *this;
    }

    INF_INLINE Vec2 operator*( const Vec2 &vec, const Mat2 &mat ) {
        return mat * vec;
    }

    INF_INLINE Mat2 operator*( const float a, Mat2 const &mat ) {
        return mat * a;
    }

    INF_INLINE Vec2 &operator*=( Vec2 &vec, const Mat2 &mat ) {
        vec = mat * vec;
        return vec;
    }

    INF_INLINE bool Mat2::compare( const Mat2 &a ) const {
        if ( mat[0].compare( a[0] ) &&
            mat[1].compare( a[1] ) ) {
                return true;
        }
        return false;
    }

    INF_INLINE bool Mat2::compare( const Mat2 &a, const float epsilon ) const {
        if ( mat[0].compare( a[0], epsilon ) &&
            mat[1].compare( a[1], epsilon ) ) {
                return true;
        }
        return false;
    }

    INF_INLINE bool Mat2::operator==( const Mat2 &a ) const {
        return compare( a );
    }

    INF_INLINE bool Mat2::operator!=( const Mat2 &a ) const {
        return !compare( a );
    }

    INF_INLINE void Mat2::zero( void ) {
        mat[0].zero();
        mat[1].zero();
    }

    INF_INLINE void Mat2::identity( void ) {
        mat[ 0 ].x = 1.0f; mat[ 0 ].y = 0.0f;
        mat[ 1 ].x = 0.0f; mat[ 1 ].y = 1.0f;
    }

    INF_INLINE bool Mat2::is_identity( const float epsilon ) const {
        return compare( IDENTITY, epsilon );
    }

    INF_INLINE bool Mat2::is_symmetric( const float epsilon ) const {
        return ( math::fabs( mat[0][1] - mat[1][0] ) < epsilon );
    }

    INF_INLINE bool Mat2::is_diagonal( const float epsilon ) const {
        if ( math::fabs( mat[0][1] ) > epsilon ||
            math::fabs( mat[1][0] ) > epsilon ) {
                return false;
        }
        return true;
    }

    INF_INLINE float Mat2::trace( void ) const {
        return ( mat[0][0] + mat[1][1] );
    }

    INF_INLINE float Mat2::determinant( void ) const {
        return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
    }

    INF_INLINE Mat2 Mat2::transpose( void ) const {
        return Mat2(	mat[0][0], mat[1][0],
            mat[0][1], mat[1][1] );
    }

    INF_INLINE Mat2 &Mat2::transpose_self( void ) {
        float tmp;

        tmp = mat[0][1];
        mat[0][1] = mat[1][0];
        mat[1][0] = tmp;

        return *this;
    }

    INF_INLINE Mat2 Mat2::inverse( void ) const {
        Mat2 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_self();
        assert( r );
#else
        invMat.inverse_self();
#endif
        return invMat;
    }

    INF_INLINE Mat2 Mat2::inverse_fast( void ) const {
        Mat2 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_fast_self();
        assert( r );
#else
        invMat.inverse_fast_self();
#endif
        return invMat;
    }

    INF_INLINE int Mat2::get_dimension( void ) const {
        return 4;
    }

    INF_INLINE int Mat2::get_square_dimension( void ) const {
        return 2;
    }

    INF_INLINE const float *Mat2::to_float_ptr( void ) const {
        return mat[0].to_float_ptr();
    }

    INF_INLINE float *Mat2::to_float_ptr( void ) {
        return mat[0].to_float_ptr();
    }

    INF_INLINE void Mat2::rotation( float angle ) {
        float s;
        float c;
        math::sincos( angle, s, c );

        mat[ 0 ].set( c, s );
        mat[ 1 ].set( -s, c );
    }

    //===============================================================
    //
    //	Mat3 - 3x3 matrix
    //
    //	NOTE:	matrix is column-major - m[ column, line ]
    //
    //===============================================================

    class Mat3 {
    public:
        Mat3( void );
        explicit Mat3( const Vec3 &x, const Vec3 &y, const Vec3 &z );
        explicit Mat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz );
        explicit Mat3( const float src[ 3 ][ 3 ] );

        const Vec3 &	operator[]( int index ) const;
        Vec3 &		operator[]( int index );
        Mat3			operator-() const;
        Mat3			operator*( const float a ) const;
        Vec3			operator*( const Vec3 &vec ) const;
        Mat3			operator*( const Mat3 &a ) const;
        // RAVEN BEGIN
        // jscott: multiply by the transpose
        Vec3			operator/( const Vec3 &vec ) const;
        Mat3			operator/( const Mat3 &a ) const;
        // RAVEN END
        Mat3			operator+( const Mat3 &a ) const;
        Mat3			operator-( const Mat3 &a ) const;
        Mat3&			operator*=( const float a );
        Mat3&			operator*=( const Mat3 &a );
        Mat3&			operator+=( const Mat3 &a );
        Mat3&			operator-=( const Mat3 &a );
        Mat3&			operator=( const Mat3 &a );

        friend Mat3	operator*( const float a, const Mat3 &mat );
        friend Vec3	operator*( const Vec3 &vec, const Mat3 &mat );
        friend Vec3 &	operator*=( Vec3 &vec, const Mat3 &mat );

        bool			compare( const Mat3 &a ) const;						// exact compare, no epsilon
        bool			compare( const Mat3 &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==( const Mat3 &a ) const;					// exact compare, no epsilon
        bool			operator!=( const Mat3 &a ) const;					// exact compare, no epsilon

        void			zero( void );
        void			identity( void );
        bool			is_identity( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_symmetric( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_diagonal( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_rotated( void ) const;

        void			project_vector( const Vec3 &src, Vec3 &dst ) const;
        void			unproject_vector( const Vec3 &src, Vec3 &dst ) const;

        bool			fix_degeneracies( void );									// fix degenerate axial cases
        bool			fix_denormals( float epsilon = MATRIX_EPSILON );				// change tiny numbers to zero

        float			trace( void ) const;
        float			determinant( void ) const;
        Mat3			ortho_normalize( void ) const;
        Mat3 &		    ortho_normalize_self( void );
        Mat3			transpose( void ) const;	// returns transpose
        Mat3 &		    transpose_self( void );
        Mat3			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
        bool			inverse_self( void );		// returns false if determinant is zero
        Mat3			inverse_fast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
        bool			inverse_fast_self( void );	// returns false if determinant is zero
        Mat3			transpose_multiply( const Mat3 &b ) const;
        Vec3			transpose_multiply( const Vec3 &vec ) const;

        Mat3			inertia_translate( const float mass, const Vec3 &centerOfMass, const Vec3 &translation ) const;
        Mat3 &		    inertia_translate_self( const float mass, const Vec3 &centerOfMass, const Vec3 &translation );
        Mat3			inertia_rotate( const Mat3 &rotation ) const;
        Mat3 &		    inertia_rotate_self( const Mat3 &rotation );

        int				get_dimension( void ) const;
        int				get_square_dimension( void ) const;

        Angles		    to_Angles( void ) const;
        Quat			to_Quat( void ) const;
        CQuat			to_CQuat( void ) const;
        Rotation		to_Rotation( void ) const;
        Mat4			to_Mat4( void ) const;
        Vec3			to_angular_velocity( void ) const;
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        Mat3			to_maya( void ) const;
        Mat3&			to_maya_self( void );
        Mat3			from_maya( void ) const;
        Mat3&			from_maya_self( void );

        Angles		    to_Angles_maya( void ) const;

        friend void		transpose_multiply( const Mat3 &inv, const Mat3 &b, Mat3 &dst );
        friend Mat3	    skew_symmetric( Vec3 const &src );

        static Mat3 ZERO;
        static Mat3 IDENTITY;
    private:
        Vec3			mat[ 3 ];
    };


    INF_INLINE Mat3::Mat3( void ) {
    }

    INF_INLINE Mat3::Mat3( const Vec3 &x, const Vec3 &y, const Vec3 &z ) {
        mat[ 0 ].x = x.x; mat[ 0 ].y = x.y; mat[ 0 ].z = x.z;
        mat[ 1 ].x = y.x; mat[ 1 ].y = y.y; mat[ 1 ].z = y.z;
        mat[ 2 ].x = z.x; mat[ 2 ].y = z.y; mat[ 2 ].z = z.z;
    }

    INF_INLINE Mat3::Mat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz ) {
        mat[ 0 ].x = xx; mat[ 0 ].y = xy; mat[ 0 ].z = xz;
        mat[ 1 ].x = yx; mat[ 1 ].y = yy; mat[ 1 ].z = yz;
        mat[ 2 ].x = zx; mat[ 2 ].y = zy; mat[ 2 ].z = zz;
    }

    INF_INLINE Mat3::Mat3( const float src[ 3 ][ 3 ] ) {
        memory_copy( mat, src, 3 * 3 * sizeof( float ) );
    }

    INF_INLINE const Vec3 &Mat3::operator[]( int index ) const {
        //assert( ( index >= 0 ) && ( index < 3 ) );
        return mat[ index ];
    }

    INF_INLINE Vec3 &Mat3::operator[]( int index ) {
        //assert( ( index >= 0 ) && ( index < 3 ) );
        return mat[ index ];
    }

    INF_INLINE Mat3 Mat3::operator-( void ) const {
        return Mat3(	-mat[0][0], -mat[0][1], -mat[0][2],
            -mat[1][0], -mat[1][1], -mat[1][2],
            -mat[2][0], -mat[2][1], -mat[2][2] );
    }

    INF_INLINE Vec3 Mat3::operator*( const Vec3 &vec ) const {
        return Vec3(
            mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
            mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
            mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
    }

    INF_INLINE Mat3 Mat3::operator*( const Mat3 &a ) const {
        int i, j;
        const float *m1Ptr, *m2Ptr;
        float *dstPtr;
        Mat3 dst;

        m1Ptr = reinterpret_cast<const float *>(this);
        m2Ptr = reinterpret_cast<const float *>(&a);
        dstPtr = reinterpret_cast<float *>(&dst);

        for ( i = 0; i < 3; i++ ) {
            for ( j = 0; j < 3; j++ ) {
                *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
                + m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
                + m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
                dstPtr++;
            }
            m1Ptr += 3;
        }
        return dst;
    }

    // RAVEN BEGIN
    // jscott: divide is overridden to multiply by transpose
    INF_INLINE Vec3 Mat3::operator/( const Vec3 &vec ) const 
    {
        return( Vec3(
            mat[0].x * vec.x + mat[0].y * vec.y + mat[0].z * vec.z,
            mat[1].x * vec.x + mat[1].y * vec.y + mat[1].z * vec.z,
            mat[2].x * vec.x + mat[2].y * vec.y + mat[2].z * vec.z ) );
    }

    INF_INLINE Mat3& Mat3::operator=( const Mat3 &a ) {
        memory_copy( mat[ 0 ].to_float_ptr(), a.mat[ 0 ].to_float_ptr(), sizeof( mat ) );
        return *this;
    }

    INF_INLINE Mat3 Mat3::operator/( const Mat3 &a ) const 
    {
        Mat3		dst;

        dst[0].x = mat[0].x * a.mat[0].x + mat[0].y * a.mat[0].y + mat[0].z * a.mat[0].z;
        dst[0].y = mat[0].x * a.mat[1].x + mat[0].y * a.mat[1].y + mat[0].z * a.mat[1].z;
        dst[0].z = mat[0].x * a.mat[2].x + mat[0].y * a.mat[2].y + mat[0].z * a.mat[2].z;

        dst[1].x = mat[1].x * a.mat[0].x + mat[1].y * a.mat[0].y + mat[1].z * a.mat[0].z;
        dst[1].y = mat[1].x * a.mat[1].x + mat[1].y * a.mat[1].y + mat[1].z * a.mat[1].z;
        dst[1].z = mat[1].x * a.mat[2].x + mat[1].y * a.mat[2].y + mat[1].z * a.mat[2].z;

        dst[2].x = mat[2].x * a.mat[0].x + mat[2].y * a.mat[0].y + mat[2].z * a.mat[0].z;
        dst[2].y = mat[2].x * a.mat[1].x + mat[2].y * a.mat[1].y + mat[2].z * a.mat[1].z;
        dst[2].z = mat[2].x * a.mat[2].x + mat[2].y * a.mat[2].y + mat[2].z * a.mat[2].z;

        return( dst );
    }
    // RAVEN END

    INF_INLINE Mat3 Mat3::operator*( const float a ) const {
        return Mat3(
            mat[0].x * a, mat[0].y * a, mat[0].z * a,
            mat[1].x * a, mat[1].y * a, mat[1].z * a,
            mat[2].x * a, mat[2].y * a, mat[2].z * a );
    }

    INF_INLINE Mat3 Mat3::operator+( const Mat3 &a ) const {
        return Mat3(
            mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z,
            mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z,
            mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z );
    }

    INF_INLINE Mat3 Mat3::operator-( const Mat3 &a ) const {
        return Mat3(
            mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z,
            mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z,
            mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z );
    }

    INF_INLINE Mat3 &Mat3::operator*=( const float a ) {
        mat[0].x *= a; mat[0].y *= a; mat[0].z *= a;
        mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; 
        mat[2].x *= a; mat[2].y *= a; mat[2].z *= a;

        return *this;
    }

    INF_INLINE Mat3 &Mat3::operator*=( const Mat3 &a ) {
        int i, j;
        const float *m2Ptr;
        float *m1Ptr, dst[3];

        m1Ptr = reinterpret_cast<float *>(this);
        m2Ptr = reinterpret_cast<const float *>(&a);

        for ( i = 0; i < 3; i++ ) {
            for ( j = 0; j < 3; j++ ) {
                dst[j]  = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
                + m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
                + m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
            }
            m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
            m1Ptr += 3;
        }
        return *this;
    }

    INF_INLINE Mat3 &Mat3::operator+=( const Mat3 &a ) {
        mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z;
        mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z;
        mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z;

        return *this;
    }

    INF_INLINE Mat3 &Mat3::operator-=( const Mat3 &a ) {
        mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z;
        mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z;
        mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z;

        return *this;
    }

    INF_INLINE Vec3 operator*( const Vec3 &vec, const Mat3 &mat ) {
        return mat * vec;
    }

    INF_INLINE Mat3 operator*( const float a, const Mat3 &mat ) {
        return mat * a;
    }

    INF_INLINE Vec3 &operator*=( Vec3 &vec, const Mat3 &mat ) {
        float x = mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z;
        float y = mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z;
        vec.z = mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z;
        vec.x = x;
        vec.y = y;
        return vec;
    }

    INF_INLINE bool Mat3::compare( const Mat3 &a ) const {
        if ( mat[0].compare( a[0] ) &&
            mat[1].compare( a[1] ) &&
            mat[2].compare( a[2] ) ) {
                return true;
        }
        return false;
    }

    INF_INLINE bool Mat3::compare( const Mat3 &a, const float epsilon ) const {
        if ( mat[0].compare( a[0], epsilon ) &&
            mat[1].compare( a[1], epsilon ) &&
            mat[2].compare( a[2], epsilon ) ) {
                return true;
        }
        return false;
    }

    INF_INLINE bool Mat3::operator==( const Mat3 &a ) const {
        return compare( a );
    }

    INF_INLINE bool Mat3::operator!=( const Mat3 &a ) const {
        return !compare( a );
    }

    INF_INLINE void Mat3::zero( void ) {
        memory_set( mat, 0, sizeof( Mat3 ) );
    }

    INF_INLINE void Mat3::identity( void ) {
        mat[ 0 ].x = 1.0f; mat[ 0 ].y = 0.0f; mat[ 0 ].z = 0.0f;
        mat[ 1 ].x = 0.0f; mat[ 1 ].y = 1.0f; mat[ 1 ].z = 0.0f;
        mat[ 2 ].x = 0.0f; mat[ 2 ].y = 0.0f; mat[ 2 ].z = 1.0f;
    }

    INF_INLINE bool Mat3::is_identity( const float epsilon ) const {
        return compare( IDENTITY, epsilon );
    }

    INF_INLINE bool Mat3::is_symmetric( const float epsilon ) const {
        if ( math::fabs( mat[0][1] - mat[1][0] ) > epsilon ) {
            return false;
        }
        if ( math::fabs( mat[0][2] - mat[2][0] ) > epsilon ) {
            return false;
        }
        if ( math::fabs( mat[1][2] - mat[2][1] ) > epsilon ) {
            return false;
        }
        return true;
    }

    INF_INLINE bool Mat3::is_diagonal( const float epsilon ) const {
        if ( math::fabs( mat[0][1] ) > epsilon ||
            math::fabs( mat[0][2] ) > epsilon ||
            math::fabs( mat[1][0] ) > epsilon ||
            math::fabs( mat[1][2] ) > epsilon ||
            math::fabs( mat[2][0] ) > epsilon ||
            math::fabs( mat[2][1] ) > epsilon ) {
                return false;
        }
        return true;
    }

    INF_INLINE bool Mat3::is_rotated( void ) const {
        // NOTE: assumes the 3x3 matrix is orthonormal
        return ( mat[0][0] != 1.0f || mat[1][1] != 1.0f || mat[2][2] != 1.0f );
    }

    INF_INLINE void Mat3::project_vector( const Vec3 &src, Vec3 &dst ) const {
        dst.x = src * mat[ 0 ];
        dst.y = src * mat[ 1 ];
        dst.z = src * mat[ 2 ];
    }

    INF_INLINE void Mat3::unproject_vector( const Vec3 &src, Vec3 &dst ) const {
        dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z;
    }

    INF_INLINE bool Mat3::fix_degeneracies( void ) {
        bool r = mat[0].fix_degenerate_normal();
        r |= mat[1].fix_degenerate_normal();
        r |= mat[2].fix_degenerate_normal();
        return r;
    }

    INF_INLINE bool Mat3::fix_denormals( float epsilon ) {
        bool r = mat[0].fix_denormals( epsilon );
        r |= mat[1].fix_denormals( epsilon );
        r |= mat[2].fix_denormals( epsilon );
        return r;
    }

    INF_INLINE float Mat3::trace( void ) const {
        return ( mat[0][0] + mat[1][1] + mat[2][2] );
    }

    INF_INLINE Mat3 Mat3::ortho_normalize( void ) const {
        Mat3 ortho;

        ortho = *this;
        ortho[ 0 ].normalize();
        ortho[ 2 ].cross( mat[ 0 ], mat[ 1 ] );
        ortho[ 2 ].normalize();
        ortho[ 1 ].cross( mat[ 2 ], mat[ 0 ] );
        ortho[ 1 ].normalize();
        return ortho;
    }

    INF_INLINE Mat3 &Mat3::ortho_normalize_self( void ) {
        mat[ 0 ].normalize();
        mat[ 2 ].cross( mat[ 0 ], mat[ 1 ] );
        mat[ 2 ].normalize();
        mat[ 1 ].cross( mat[ 2 ], mat[ 0 ] );
        mat[ 1 ].normalize();
        return *this;
    }

    INF_INLINE Mat3 Mat3::transpose( void ) const {
        return Mat3(	mat[0][0], mat[1][0], mat[2][0],
            mat[0][1], mat[1][1], mat[2][1],
            mat[0][2], mat[1][2], mat[2][2] );
    }

    INF_INLINE Mat3 &Mat3::transpose_self( void ) {
        float tmp0, tmp1, tmp2;

        tmp0 = mat[0][1];
        mat[0][1] = mat[1][0];
        mat[1][0] = tmp0;
        tmp1 = mat[0][2];
        mat[0][2] = mat[2][0];
        mat[2][0] = tmp1;
        tmp2 = mat[1][2];
        mat[1][2] = mat[2][1];
        mat[2][1] = tmp2;

        return *this;
    }

    INF_INLINE Mat3 Mat3::inverse( void ) const {
        Mat3 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_self();
        assert( r );
#else
        invMat.inverse_self();
#endif
        return invMat;
    }

    INF_INLINE Mat3 Mat3::inverse_fast( void ) const {
        Mat3 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_fast_self();
        assert( r );
#else
        invMat.inverse_fast_self();
#endif
        return invMat;
    }

    INF_INLINE Mat3 Mat3::transpose_multiply( const Mat3 &b ) const {
        return Mat3(	mat[0].x * b[0].x + mat[1].x * b[1].x + mat[2].x * b[2].x,
            mat[0].x * b[0].y + mat[1].x * b[1].y + mat[2].x * b[2].y,
            mat[0].x * b[0].z + mat[1].x * b[1].z + mat[2].x * b[2].z,
            mat[0].y * b[0].x + mat[1].y * b[1].x + mat[2].y * b[2].x,
            mat[0].y * b[0].y + mat[1].y * b[1].y + mat[2].y * b[2].y,
            mat[0].y * b[0].z + mat[1].y * b[1].z + mat[2].y * b[2].z,
            mat[0].z * b[0].x + mat[1].z * b[1].x + mat[2].z * b[2].x,
            mat[0].z * b[0].y + mat[1].z * b[1].y + mat[2].z * b[2].y,
            mat[0].z * b[0].z + mat[1].z * b[1].z + mat[2].z * b[2].z );
    }

    INF_INLINE Vec3 Mat3::transpose_multiply( const Vec3 &vec ) const {
        return Vec3(
            mat[0].x * vec.x + mat[0].y * vec.y + mat[0].z * vec.z,
            mat[1].x * vec.x + mat[1].y * vec.y + mat[1].z * vec.z,
            mat[2].x * vec.x + mat[2].y * vec.y + mat[2].z * vec.z );
    }

    INF_INLINE void transpose_multiply( const Mat3 &transpose, const Mat3 &b, Mat3 &dst ) {
        dst[0].x = transpose[0].x * b[0].x + transpose[1].x * b[1].x + transpose[2].x * b[2].x;
        dst[0].y = transpose[0].x * b[0].y + transpose[1].x * b[1].y + transpose[2].x * b[2].y;
        dst[0].z = transpose[0].x * b[0].z + transpose[1].x * b[1].z + transpose[2].x * b[2].z;
        dst[1].x = transpose[0].y * b[0].x + transpose[1].y * b[1].x + transpose[2].y * b[2].x;
        dst[1].y = transpose[0].y * b[0].y + transpose[1].y * b[1].y + transpose[2].y * b[2].y;
        dst[1].z = transpose[0].y * b[0].z + transpose[1].y * b[1].z + transpose[2].y * b[2].z;
        dst[2].x = transpose[0].z * b[0].x + transpose[1].z * b[1].x + transpose[2].z * b[2].x;
        dst[2].y = transpose[0].z * b[0].y + transpose[1].z * b[1].y + transpose[2].z * b[2].y;
        dst[2].z = transpose[0].z * b[0].z + transpose[1].z * b[1].z + transpose[2].z * b[2].z;
    }

    INF_INLINE Mat3 skew_symmetric( Vec3 const &src ) {
        return Mat3( 0.0f, -src.z,  src.y, src.z,   0.0f, -src.x, -src.y,  src.x,   0.0f );
    }

    INF_INLINE int Mat3::get_dimension( void ) const {
        return 9;
    }

    INF_INLINE int Mat3::get_square_dimension( void ) const {
        return 3;
    }

    INF_INLINE const float *Mat3::to_float_ptr( void ) const {
        return mat[0].to_float_ptr();
    }

    INF_INLINE float *Mat3::to_float_ptr( void ) {
        return mat[0].to_float_ptr();
    }

    INF_INLINE Mat3 Mat3::to_maya( void ) const {
        Mat3 matMaya;

        matMaya = *this;
        matMaya.to_maya_self();
        return matMaya;
    }

    INF_INLINE Mat3& Mat3::to_maya_self( void ) {
        iswap( mat[ 0 ][ 2 ], mat[ 0 ][ 1 ] );
        mat[ 0 ][ 2 ] = -mat[ 0 ][ 2 ];

        iswap( mat[ 1 ][ 2 ], mat[ 1 ][ 1 ] );
        mat[ 1 ][ 2 ] = -mat[ 1 ][ 2 ];

        iswap( mat[ 2 ][ 1 ], mat[ 2 ][ 2 ] );
        mat[ 2 ][ 2 ] = -mat[ 2 ][ 2 ];

        return (*this);
    }

    INF_INLINE Mat3 Mat3::from_maya( void ) const {
        Mat3 matId;

        matId = *this;
        matId.from_maya_self();
        return matId;
    }

    INF_INLINE Mat3& Mat3::from_maya_self( void ) {
        iswap( mat[ 0 ][ 2 ], mat[ 0 ][ 1 ] );
        mat[ 0 ][ 1 ] = -mat[ 0 ][ 1 ];

        iswap( mat[ 1 ][ 2 ], mat[ 1 ][ 1 ] );
        mat[ 1 ][ 1 ] = -mat[ 1 ][ 1 ];

        iswap( mat[ 2 ][ 1 ], mat[ 2 ][ 2 ] );
        mat[ 2 ][ 1 ] = -mat[ 2 ][ 1 ];

        return (*this);
    }

    //===============================================================
    //
    //	Mat4 - 4x4 matrix
    //
    //===============================================================

    class Mat4 {
    public:
        Mat4( void );
        explicit Mat4( const Vec4 &x, const Vec4 &y, const Vec4 &z, const Vec4 &w );
        explicit Mat4(const float xx, const float xy, const float xz, const float xw,
            const float yx, const float yy, const float yz, const float yw,
            const float zx, const float zy, const float zz, const float zw,
            const float wx, const float wy, const float wz, const float ww );
        explicit Mat4( const Mat3 &rotation, const Vec3 &translation );
        explicit Mat4( const float src[ 4 ][ 4 ] );

        const Vec4 &	operator[]( int index ) const;
        Vec4 &		operator[]( int index );
        Mat4			operator*( const float a ) const;
        Vec4			operator*( const Vec4 &vec ) const;
        Vec3			operator*( const Vec3 &vec ) const;
        Mat4			operator*( const Mat4 &a ) const;
        Mat4			operator+( const Mat4 &a ) const;
        Mat4			operator-( const Mat4 &a ) const;
        Mat4 &		operator*=( const float a );
        Mat4 &		operator*=( const Mat4 &a );
        Mat4 &		operator+=( const Mat4 &a );
        Mat4 &		operator-=( const Mat4 &a );

        friend Mat4	operator*( const float a, const Mat4 &mat );
        friend Vec4	operator*( const Vec4 &vec, const Mat4 &mat );
        friend Vec3	operator*( const Vec3 &vec, const Mat4 &mat );
        friend Vec4 &	operator*=( Vec4 &vec, const Mat4 &mat );
        friend Vec3 &	operator*=( Vec3 &vec, const Mat4 &mat );

        bool			compare( const Mat4 &a ) const;						// exact compare, no epsilon
        bool			compare( const Mat4 &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==( const Mat4 &a ) const;					// exact compare, no epsilon
        bool			operator!=( const Mat4 &a ) const;					// exact compare, no epsilon

        void			zero( void );
        void			identity( void );
        bool			is_identity( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_symmetric( const float epsilon = MATRIX_EPSILON ) const;
        bool			is_diagonal( const float epsilon = MATRIX_EPSILON ) const;

        void			project_vector( const Vec4 &src, Vec4 &dst ) const;
        void			unproject_vector( const Vec4 &src, Vec4 &dst ) const;

        float			trace( void ) const;
        float			determinant( void ) const;
        Mat4			transpose( void ) const;	// returns transpose
        Mat4 &			transpose_self( void );
        Mat4			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
        bool			inverse_self( void );		// returns false if determinant is zero
        Mat4			inverse_fast( void ) const;	// returns the inverse ( m * m.inverse() = identity ) all Positive Definite matrices and P-matrices should be fine
        bool			inverse_fast_self( void );	// returns false if determinant is zero all Positive Definite matrices and P-matrices should be fine
        Mat4			transpose_multiply( const Mat4 &b ) const;

        int				get_dimension( void ) const;
        int				get_square_dimension( void ) const;

        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        static Mat4 ZERO;
        static Mat4 IDENTITY;
    private:
        Vec4			mat[ 4 ];
    };


    INF_INLINE Mat4::Mat4( void ) {
    }

    INF_INLINE Mat4::Mat4( const Vec4 &x, const Vec4 &y, const Vec4 &z, const Vec4 &w ) {
        mat[ 0 ] = x;
        mat[ 1 ] = y;
        mat[ 2 ] = z;
        mat[ 3 ] = w;
    }

    INF_INLINE Mat4::Mat4( const float xx, const float xy, const float xz, const float xw,
        const float yx, const float yy, const float yz, const float yw,
        const float zx, const float zy, const float zz, const float zw,
        const float wx, const float wy, const float wz, const float ww ) {
            mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
            mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
            mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
            mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
    }

    INF_INLINE Mat4::Mat4( const Mat3 &rotation, const Vec3 &translation ) {
        // NOTE: Mat3 is transposed because it is column-major
        mat[ 0 ][ 0 ] = rotation[0][0];
        mat[ 0 ][ 1 ] = rotation[1][0];
        mat[ 0 ][ 2 ] = rotation[2][0];
        mat[ 0 ][ 3 ] = translation[0];
        mat[ 1 ][ 0 ] = rotation[0][1];
        mat[ 1 ][ 1 ] = rotation[1][1];
        mat[ 1 ][ 2 ] = rotation[2][1];
        mat[ 1 ][ 3 ] = translation[1];
        mat[ 2 ][ 0 ] = rotation[0][2];
        mat[ 2 ][ 1 ] = rotation[1][2];
        mat[ 2 ][ 2 ] = rotation[2][2];
        mat[ 2 ][ 3 ] = translation[2];
        mat[ 3 ][ 0 ] = 0.0f;
        mat[ 3 ][ 1 ] = 0.0f;
        mat[ 3 ][ 2 ] = 0.0f;
        mat[ 3 ][ 3 ] = 1.0f;
    }

    INF_INLINE Mat4::Mat4( const float src[ 4 ][ 4 ] ) {
        memory_copy( mat, src, 4 * 4 * sizeof( float ) );
    }

    INF_INLINE const Vec4 &Mat4::operator[]( int index ) const {
        //assert( ( index >= 0 ) && ( index < 4 ) );
        return mat[ index ];
    }

    INF_INLINE Vec4 &Mat4::operator[]( int index ) {
        //assert( ( index >= 0 ) && ( index < 4 ) );
        return mat[ index ];
    }

    INF_INLINE Mat4 Mat4::operator*( const float a ) const {
        return Mat4(
            mat[0].x * a, mat[0].y * a, mat[0].z * a, mat[0].w * a,
            mat[1].x * a, mat[1].y * a, mat[1].z * a, mat[1].w * a,
            mat[2].x * a, mat[2].y * a, mat[2].z * a, mat[2].w * a,
            mat[3].x * a, mat[3].y * a, mat[3].z * a, mat[3].w * a );
    }

    INF_INLINE Vec4 Mat4::operator*( const Vec4 &vec ) const {
        return Vec4(
            mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w * vec.w,
            mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w * vec.w,
            mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w * vec.w,
            mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w * vec.w );
    }

    INF_INLINE Vec3 Mat4::operator*( const Vec3 &vec ) const {
        float s = mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w;
        if ( s == 0.0f ) {
            return Vec3( 0.0f, 0.0f, 0.0f );
        }
        if ( s == 1.0f ) {
            return Vec3(
                mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w,
                mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w,
                mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w );
        }
        else {
            float invS = 1.0f / s;
            return Vec3(
                (mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w) * invS,
                (mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w) * invS,
                (mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w) * invS );
        }
    }

    INF_INLINE Mat4 Mat4::operator*( const Mat4 &a ) const {
        int i, j;
        const float *m1Ptr, *m2Ptr;
        float *dstPtr;
        Mat4 dst;

        m1Ptr = reinterpret_cast<const float *>(this);
        m2Ptr = reinterpret_cast<const float *>(&a);
        dstPtr = reinterpret_cast<float *>(&dst);

        for ( i = 0; i < 4; i++ ) {
            for ( j = 0; j < 4; j++ ) {
                *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 4 + j ]
                + m1Ptr[1] * m2Ptr[ 1 * 4 + j ]
                + m1Ptr[2] * m2Ptr[ 2 * 4 + j ]
                + m1Ptr[3] * m2Ptr[ 3 * 4 + j ];
                dstPtr++;
            }
            m1Ptr += 4;
        }
        return dst;
    }

    INF_INLINE Mat4 Mat4::operator+( const Mat4 &a ) const {
        return Mat4( 
            mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z, mat[0].w + a[0].w,
            mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z, mat[1].w + a[1].w,
            mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z, mat[2].w + a[2].w,
            mat[3].x + a[3].x, mat[3].y + a[3].y, mat[3].z + a[3].z, mat[3].w + a[3].w );
    }

    INF_INLINE Mat4 Mat4::operator-( const Mat4 &a ) const {
        return Mat4( 
            mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z, mat[0].w - a[0].w,
            mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z, mat[1].w - a[1].w,
            mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z, mat[2].w - a[2].w,
            mat[3].x - a[3].x, mat[3].y - a[3].y, mat[3].z - a[3].z, mat[3].w - a[3].w );
    }

    INF_INLINE Mat4 &Mat4::operator*=( const float a ) {
        mat[0].x *= a; mat[0].y *= a; mat[0].z *= a; mat[0].w *= a;
        mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; mat[1].w *= a;
        mat[2].x *= a; mat[2].y *= a; mat[2].z *= a; mat[2].w *= a;
        mat[3].x *= a; mat[3].y *= a; mat[3].z *= a; mat[3].w *= a;
        return *this;
    }

    INF_INLINE Mat4 &Mat4::operator*=( const Mat4 &a ) {
        *this = (*this) * a;
        return *this;
    }

    INF_INLINE Mat4 &Mat4::operator+=( const Mat4 &a ) {
        mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z; mat[0].w += a[0].w;
        mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z; mat[1].w += a[1].w;
        mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z; mat[2].w += a[2].w;
        mat[3].x += a[3].x; mat[3].y += a[3].y; mat[3].z += a[3].z; mat[3].w += a[3].w;
        return *this;
    }

    INF_INLINE Mat4 &Mat4::operator-=( const Mat4 &a ) {
        mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z; mat[0].w -= a[0].w;
        mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z; mat[1].w -= a[1].w;
        mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z; mat[2].w -= a[2].w;
        mat[3].x -= a[3].x; mat[3].y -= a[3].y; mat[3].z -= a[3].z; mat[3].w -= a[3].w;
        return *this;
    }

    INF_INLINE Mat4 operator*( const float a, const Mat4 &mat ) {
        return mat * a;
    }

    INF_INLINE Vec4 operator*( const Vec4 &vec, const Mat4 &mat ) {
        return mat * vec;
    }

    INF_INLINE Vec3 operator*( const Vec3 &vec, const Mat4 &mat ) {
        return mat * vec;
    }

    INF_INLINE Vec4 &operator*=( Vec4 &vec, const Mat4 &mat ) {
        vec = mat * vec;
        return vec;
    }

    INF_INLINE Vec3 &operator*=( Vec3 &vec, const Mat4 &mat ) {
        vec = mat * vec;
        return vec;
    }

    INF_INLINE bool Mat4::compare( const Mat4 &a ) const {
        dword i;
        const float *ptr1, *ptr2;

        ptr1 = reinterpret_cast<const float *>(mat);
        ptr2 = reinterpret_cast<const float *>(a.mat);
        for ( i = 0; i < 4*4; i++ ) {
            if ( ptr1[i] != ptr2[i] ) {
                return false;
            }
        }
        return true;
    }

    INF_INLINE bool Mat4::compare( const Mat4 &a, const float epsilon ) const {
        dword i;
        const float *ptr1, *ptr2;

        ptr1 = reinterpret_cast<const float *>(mat);
        ptr2 = reinterpret_cast<const float *>(a.mat);
        for ( i = 0; i < 4*4; i++ ) {
            if ( math::fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
                return false;
            }
        }
        return true;
    }

    INF_INLINE bool Mat4::operator==( const Mat4 &a ) const {
        return compare( a );
    }

    INF_INLINE bool Mat4::operator!=( const Mat4 &a ) const {
        return !compare( a );
    }

    INF_INLINE void Mat4::zero( void ) {
        memory_set( mat, 0, sizeof( Mat4 ) );
    }

    INF_INLINE void Mat4::identity( void ) {
        mat[ 0 ].x = 1.0f; mat[ 0 ].y = 0.0f; mat[ 0 ].z = 0.0f; mat[ 0 ].w = 0.0f;
        mat[ 1 ].x = 0.0f; mat[ 1 ].y = 1.0f; mat[ 1 ].z = 0.0f; mat[ 1 ].w = 0.0f;
        mat[ 2 ].x = 0.0f; mat[ 2 ].y = 0.0f; mat[ 2 ].z = 1.0f; mat[ 2 ].w = 0.0f;
        mat[ 3 ].x = 0.0f; mat[ 3 ].y = 0.0f; mat[ 3 ].z = 0.0f; mat[ 3 ].w = 1.0f;
    }

    INF_INLINE bool Mat4::is_identity( const float epsilon ) const {
        return compare( IDENTITY, epsilon );
    }

    INF_INLINE bool Mat4::is_symmetric( const float epsilon ) const {
        for ( int i = 1; i < 4; i++ ) {
            for ( int j = 0; j < i; j++ ) {
                if ( math::fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
                    return false;
                }
            }
        }
        return true;
    }

    INF_INLINE bool Mat4::is_diagonal( const float epsilon ) const {
        for ( int i = 0; i < 4; i++ ) {
            for ( int j = 0; j < 4; j++ ) {
                if ( i != j && math::fabs( mat[i][j] ) > epsilon ) {
                    return false;
                }
            }
        }
        return true;
    }

    INF_INLINE void Mat4::project_vector( const Vec4 &src, Vec4 &dst ) const {
        dst.x = src * mat[ 0 ];
        dst.y = src * mat[ 1 ];
        dst.z = src * mat[ 2 ];
        dst.w = src * mat[ 3 ];
    }

    INF_INLINE void Mat4::unproject_vector( const Vec4 &src, Vec4 &dst ) const {
        dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z + mat[ 3 ] * src.w;
    }

    INF_INLINE float Mat4::trace( void ) const {
        return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] );
    }

    INF_INLINE Mat4 Mat4::inverse( void ) const {
        Mat4 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_self();
        assert( r );
#else
        invMat.inverse_self();
#endif
        return invMat;
    }

    INF_INLINE Mat4 Mat4::inverse_fast( void ) const {
        Mat4 invMat;

        invMat = *this;
#ifndef NDEBUG
        int r = invMat.inverse_fast_self();
        assert( r );
#else
        invMat.inverse_fast_self();
#endif
        return invMat;
    }

    INF_INLINE Mat4 Mat3::to_Mat4( void ) const {
        // NOTE: Mat3 is transposed because it is column-major
        return Mat4(	mat[0][0],	mat[1][0],	mat[2][0],	0.0f,
            mat[0][1],	mat[1][1],	mat[2][1],	0.0f,
            mat[0][2],	mat[1][2],	mat[2][2],	0.0f,
            0.0f,		0.0f,		0.0f,		1.0f );
    }

    INF_INLINE int Mat4::get_dimension( void ) const {
        return 16;
    }

    INF_INLINE const float *Mat4::to_float_ptr( void ) const {
        return mat[0].to_float_ptr();
    }

    INF_INLINE float *Mat4::to_float_ptr( void ) {
        return mat[0].to_float_ptr();
    }

}   // ::inf

#endif /* !__MATH_MATRIX_H__ */
